#include "scene.hpp"
#include <iostream>
#include <GL/gl.h>
#include <GL/glu.h>

static int global_counter = 0;
NodeNameID m_node_IDs[50];
SceneNode::SceneNode(const std::string& name)
  : m_name(name), m_parent(NULL), pick(false)
{
    //m_id = (int)this;
	m_id = global_counter;
	m_node_IDs[m_id].setNodeNameID(m_id, this);
	global_counter++;
    set_transform(Matrix4x4());
    
}

SceneNode::~SceneNode()
{
}

void SceneNode::highlight_children()
{
	if(!is_joint()) return;
	set_pick();
 	for (ChildList::const_iterator node = m_children.begin(); node != m_children.end(); node++) {
		if(!(*node)->is_joint()){ 
			(*node)->set_pick(); 
		}
	}
}

void SceneNode::set_pick()
{
	pick = !pick;
}

bool SceneNode::get_pick()
{
	return pick;
}

int SceneNode::get_id()
{
	return m_id;
}

void SceneNode::reset_joints()
{
	for (ChildList::const_iterator node = m_children.begin(); node != m_children.end(); node++) {
		if((*node)->is_joint())
			(*node)->reset_joints();
    }
}

void SceneNode::walk_gl(bool picking) const
{
  // Fill me in
  	glPushMatrix(); // save current matrix
	glMultMatrixd(get_transform().transpose().begin());
    for (ChildList::const_iterator node = m_children.begin(); node != m_children.end(); node++) {
		(*node)->walk_gl(picking);
    }
    glPopMatrix(); // restore back
}

void SceneNode::rotate(char axis, double angle)
{
  //std::cerr << "Stub: Rotate " << m_name << " around " << axis << " by " << angle << std::endl;
  // Fill me in
    angle = angle/180*M_PI;
    Vector4D row1, row2, row3, row4;
	switch( axis ){
		case 'x':
			row1 = Vector4D(1.0, 0.0, 0.0, 0.0);
			row2 = Vector4D(0.0, cos(angle), -sin(angle), 0.0);
			row3 = Vector4D(0.0, sin(angle), cos(angle), 0.0);
			row4 = Vector4D(0.0, 0.0, 0.0, 1.0);
			break;
		case 'y':
			row1 = Vector4D(cos(angle), 0.0, sin(angle), 0.0);
			row2 = Vector4D(0.0, 1.0, 0.0, 0.0);
			row3 = Vector4D(-sin(angle), 0.0, cos(angle), 0.0);
			row4 = Vector4D(0.0, 0.0, 0.0, 1.0);
			break;
		case 'z':
			row1 = Vector4D(cos(angle), -sin(angle), 0.0, 0.0);
			row2 = Vector4D(sin(angle), cos(angle), 0.0, 0.0);
			row3 = Vector4D(0.0, 0.0, 1.0, 0.0);
			row4 = Vector4D(0.0, 0.0, 0.0, 1.0);
			break;
	}
	Matrix4x4 rot = Matrix4x4(row1, row2, row3, row4);
    set_transform( get_transform() * rot );
}

void SceneNode::scale(const Vector3D& amount)
{
  //std::cerr << "Stub: Scale " << m_name << " by " << amount << std::endl;
  // Fill me in
    Vector4D row1( amount[0], 0.0, 0.0, 0.0);
	Vector4D row2( 0.0, amount[1], 0.0, 0.0);
	Vector4D row3( 0.0, 0.0, amount[2], 0.0);
	Vector4D row4( 0.0, 0.0, 0.0, 1.0);
	
	Matrix4x4 scale = Matrix4x4(row1, row2,row3, row4);
    set_transform( get_transform() * scale );
}

void SceneNode::translate(const Vector3D& amount)
{
  //std::cerr << "Stub: Translate " << m_name << " by " << amount << std::endl;
  // Fill me in
    Vector4D row1( 1.0, 0.0, 0.0, amount[0]);
	Vector4D row2( 0.0, 1.0, 0.0, amount[1]);
	Vector4D row3( 0.0, 0.0, 1.0, amount[2]);
	Vector4D row4( 0.0, 0.0, 0.0, 1.0);
	
	Matrix4x4 translate = Matrix4x4(row1, row2,row3, row4);
    set_transform( get_transform() * translate );
}

bool SceneNode::is_joint() const
{
  return false;
}

JointNode::JointNode(const std::string& name)
  : SceneNode(name)
{
}

JointNode::~JointNode()
{
}

void JointNode::walk_gl(bool picking) const
{
  // Fill me in
   	glPushMatrix(); // save current matrix
	glMultMatrixd(get_transform().transpose().begin());
    for (ChildList::const_iterator node = m_children.begin(); node != m_children.end(); node++) {
		(*node)->walk_gl(picking);
    }
    glPopMatrix(); // restore back
}

void JointNode::reset_joints()
{
	rotate_x(m_joint_x.init - current_x);
	rotate_y(m_joint_y.init - current_y);
	for (ChildList::const_iterator node = m_children.begin(); node != m_children.end(); node++) {
		if((*node)->is_joint())
			(*node)->reset_joints();
    }
}

bool JointNode::is_joint() const
{
  return true;
}

void JointNode::set_joint_x(double min, double init, double max)
{
  m_joint_x.min = min;
  m_joint_x.init = init;
  m_joint_x.max = max;
current_x = init;
}

void JointNode::set_joint_y(double min, double init, double max)
{
  m_joint_y.min = min;
  m_joint_y.init = init;
  m_joint_y.max = max;
current_y = init;
}

double JointNode::get_joint_x()
{
	return current_x;
}

double JointNode::get_joint_y()
{
	return current_y;
}

void JointNode::rotate_x(double angle)
{
	double temp_x = current_x + angle;
	if( temp_x > m_joint_x.max ){
		angle = m_joint_x.max - current_x;
		current_x = m_joint_x.max;
	}
	else if( temp_x < m_joint_x.min ){
		angle = m_joint_x.min - current_x;
		current_x = m_joint_x.min;
	}
	else{
		current_x = temp_x;
	}
	rotate('x', angle);	
}

void JointNode::rotate_y(double angle)
{
	double temp_y = current_y + angle;
	if( temp_y > m_joint_y.max ){
		angle = m_joint_y.max - current_y;
		current_y = m_joint_y.max;
	}
	else if( temp_y < m_joint_y.min ){
		angle = m_joint_y.min - current_y;
		current_y = m_joint_y.min;
	}
	else{
		current_y = temp_y;
	}
	rotate('y', angle);
}

GeometryNode::GeometryNode(const std::string& name, Primitive* primitive)
  : SceneNode(name), m_primitive(primitive)
{
}

GeometryNode::~GeometryNode()
{
}

void GeometryNode::walk_gl(bool picking) const
{
  // Fill me in
	glPushMatrix(); // save current matrix
	glMultMatrixd(get_transform().transpose().begin());
    if( m_primitive ){
		m_material->apply_gl(pick);
		if(picking){
			glPushName(m_id);
			m_primitive->walk_gl(picking);
			glPopName();
		}
		else m_primitive->walk_gl(picking);
    }
	glPopMatrix(); // restore back
}

